#include "math/linear_interpolation.h"

#include <cmath>

#include "common/math_utils.h"

namespace vehicle {
namespace common {
namespace math {

double slerp(const double a0, const double t0, const double a1, const double t1,
             const double t) {
  if (std::abs(t1 - t0) <= kMathEpsilon) {
    return NormalizeAngle(a0);
  }
  const double a0_n = NormalizeAngle(a0);
  const double a1_n = NormalizeAngle(a1);
  double d = a1_n - a0_n;
  if (d > M_PI) {
    d = d - 2 * M_PI;
  } else if (d < -M_PI) {
    d = d + 2 * M_PI;
  }

  const double r = (t - t0) / (t1 - t0);
  const double a = a0_n + d * r;
  return NormalizeAngle(a);
}

double LinearInterpolate(const double& x0, const double& x1, const double& w) {
  return (x1 - x0) * w + x0;
}
void LinearSpline::Build(const std::vector<double> dd0,
                         const std::vector<double> dd1) {
  xx0 = dd0;
  xx1 = dd1;
  num_point_ = xx0.size();
}

double LinearSpline::operator()(double x) {
  if (x < xx0.front()) {
    return xx1.front();
  };

  if (x > xx0.back()) {
    return xx1.back();
  };

  size_t i = 0;
  for (; i < num_point_; i++) {
    if (x < xx0[i]) {
      --i;
      break;
    }
  }
  double weight = (x - xx0[i]) / (xx0[i + 1] - xx0[i]);

  return LinearInterpolate(xx1[i], xx1[i + 1], weight);
}

}  // namespace math
}  // namespace common
}  // namespace ab
